function [T1,T21,T22,T3,T_first] = pose(a,b,R,h,tc1,tc2,x_tool,z_tool,y_tool,A,B,C_right,C_left)
%POSE 设定芯模各段位姿
%   本次项目所用芯模尺寸
%   a=77.5;b=a/2;R=a;h=27.795;L=155;tc1=150;tc2=5;x_tool=1780.5;z_tool=824;y_tool=-239;A=0;B=0;C_right=110;C_left=95;
l=0:h/45:h;
n=length(l);
z=a/b*sqrt(b^2-(l-h).^2).*(0<=l&l<=h);
y=sqrt(b^2-(b.*z./a).^2);
%%
%  求封头段法向角
dy=zeros(n,1);
for i=1:n
dy(i)=-(b^2*z(i)/(a^2*(-b^2*(z(i)^2/a^2 - 1))^(1/2)));
end
f=-1./dy;
oa=[0,1];
f1=zeros(n,2);
for i=1:n
f1(i,:)=[1,f(i)];
end
faxiangjiao=zeros(n,1);
for i=1:n
faxiangjiao(i)=acosd(dot(f1(i,:),oa)/(norm(f1(i,:))*norm(oa)));
end
faxiangjiao=flip(faxiangjiao)';
faxiangjiao(faxiangjiao==90);
%%
%右封头
Y1=ones(1,n)*y_tool;
X1=x_tool-tc2-y;
Z1=z_tool+z;


Pos1=[X1;Y1;Z1];
Pos1=Pos1';
Pos1=flip(Pos1);
eul1=zeros(n,3);
for i=1:n
eul1(i,:)=flipud([A B (C_right+faxiangjiao(i))]);
% eul1(i,:)=flipud([A (B+faxiangjiao(i)) C_right]);
if eul1(i,3)>180
    eul1(i,3)=180; 
end
end

T1=zeros(4,4,n);
for i=1:n 
T1(:,:,i)=transl(Pos1(i,:))*eul2tform(deg2rad(eul1(i,:)));
end
%%
%圆筒1
x21=zeros(1,tc1);
for i=1:tc1
x21(i)=i-1;
end
Y21=ones(1,tc1)*y_tool;
X21=x_tool+x21;
Z21=ones(1,tc1)*(z_tool+77.5);
Pos21=[X21;Y21;Z21]';
eul21=[A B 180];
T21=zeros(4,4,length(Pos21));
for i=1:length(Pos21)
T21(:,:,i)=transl(Pos21(i,:))*eul2tform(deg2rad(eul21));
end
T_first=transl([x_tool,y_tool-x21(1),z_tool+97.5])*eul2tform(deg2rad(eul21));
%%
%圆筒2
x22=zeros(1,tc2);
for i=1:tc2
x22(i)=i-1;
end
Y22=ones(1,tc2)*y_tool;
X22=x_tool-x22;
Z22=ones(1,tc2)*(z_tool+77.5);
Pos22=[X22;Y22;Z22]';
eul22=[A B 180];
T22=zeros(4,4,length(Pos22));
for i=1:length(Pos22)
T22(:,:,i)=transl(Pos22(i,:))*eul2tform(deg2rad(eul22));
end
%%
Y3=ones(1,n)*y_tool;
X3=flip(x_tool+tc1+y);
Z3=flip(z_tool+z);
Pos3=[X3;Y3;Z3];
Pos3=Pos3';
eul3=zeros(n,3);
for i=1:n
eul3(i,:)=flipud([A B -(C_left+faxiangjiao(i))]);
% eul3(i,3)=eul3(i,3)-(i-1)*(10/46);
if eul3(i,3)<-180
    eul3(i,3)=-180; 
end
end
T3=zeros(4,4,n);
for i=1:n
T3(:,:,i)=transl(Pos3(i,:))*eul2tform(deg2rad(eul3(i,:)));
end
end